/*
 * @FilePath: \DWARF2_V2\src\track\track.h
 * @Description:
 * @Author: baike && baike93@163.com
 * @Date: 2024-01-25
 * @LastEditors: baike && baike93@163.com
 * @LastEditTime: 2024-03-28
 * Copyright (c) 2024 by Tinyphoton (Wuhan) Technology Co., LTD, All Rights
 * Reserved.
 */
#ifndef __TRACK_H__
#define __TRACK_H__

#include <random>
#include <thread>
#include <vector>

#include "OneEuroFilter.h"
#include "PID.h"
#include "base.pb.h"
#include "cam_tele.h"
#include "cam_wide.h"
#include "focus.h"
#include "rgb_power.h"
#include "rkmedia_buffer.h"
#include "siamRpnTracker.h"
#include "track.pb.h"
#include "ws_constant.h"
#include "yolov8.h"

#define first_frame_cnt 5
#define DEBUG_TRACK
struct TrackState
{
    bool track_finish_ = false;
    bool track_param_update_ = false;
    bool track_object_moving_ = false;
    bool track_lost_ = false;
    bool track_redetect_ = false;
    bool track_first_start = false;
};

class Track
{
public:
    Track();
    ~Track();

    int handleMessage(const WsPacket &ws_packet);
    int initTrack();
    void notifyTrackState();
    StateNotify getTrackState();
    int startTrackNew(const WsPacket &ws_packet);
    int startTrackSentryMode(const WsPacket &ws_packet);

    int stopTrack(const WsPacket &ws_packet);

    void polyFit(const std::vector<cv::Point2f> &points, const int order,
                 cv::Mat *coeff);
    int convertSpeed2Pixel(float fov, int freq, float time,
                           int total_pixel_length);
    float pidKpRatio(const std::vector<float> &arr, int width, float k_thre);

private:
    void setAndNotifyTrackState(const StateNotify &state);
    void setAndNotifySentryModeState(const StateSentryModeNotify &state);
    int stopTrack();
    int trackWithImage(MEDIA_BUFFER fram_mb, cv::Rect init_rect);

    int wideDetect(detect_result_group_t *detect_result_group);
    int teleDetect(int cam_tele_width, int cam_tele_height,
                   MEDIA_BUFFER &tele_frame_mb_copy, bool &detect_success,
                   cv::Rect &init_rect);
    int checkWideDetectResult(detect_result_t det_result, int box_max_width,
                              int box_max_height, cv::Point &det_box_center_pos);
    int initSentryModeMotorPosition(double &yaw_position, double &pitch_position);
    int resetSentryModelPosition(double yaw_absolute_angle,
                                 double pitch_absolute_angle);

public:
    CamTele cam_tele_;
    CamWide cam_wide_;
    RgbPower rgb_power_;

private:
    Focus focus_;
    static std::thread pid_thread_;
};

#endif